import numpy as np
from scipy.spatial.transform import Rotation as R
import pinocchio as pin
import json
from enum import Enum

class IkRes:
    def __init__(self):
        self.status = 0
        self.itr = 0
        self.err = None
        self.jointPosRes = None
                
class legIdx(Enum):
    left = 0,
    right = 1


class Pin_KinDyn:
    def __init__(self, urdf_path):
        self.motorReachLimit = []
        self.motorName = ["J_arm_l_01", "J_arm_l_02", "J_arm_l_03", "J_arm_l_04", "J_arm_l_05",
                          "J_arm_l_06", "J_arm_l_07", "J_arm_r_01", "J_arm_r_02", "J_arm_r_03",
                          "J_arm_r_04", "J_arm_r_05", "J_arm_r_06", "J_arm_r_07",
                          "J_head_yaw", "J_head_pitch", "J_waist_pitch", "J_waist_roll", "J_waist_yaw",
                          "J_hip_l_roll", "J_hip_l_yaw", "J_hip_l_pitch", "J_knee_l_pitch",
                          "J_ankle_l_pitch", "J_ankle_l_roll", "J_hip_r_roll", "J_hip_r_yaw",
                          "J_hip_r_pitch", "J_knee_r_pitch", "J_ankle_r_pitch", "J_ankle_r_roll"]
        
        self.motorMaxTorque = np.zeros(len(self.motorName))
        self.motorMaxPos = np.zeros(len(self.motorName))
        self.motorMinPos = np.zeros(len(self.motorName))
        
        self.tauJointOld = np.zeros(len(self.motorName))
        self.urdf_path = urdf_path
        
        # Create the model
        self.model_biped = pin.buildModelFromUrdf(urdf_path, pin.JointModelFreeFlyer())
        # self.model_biped = pin.buildModelFromUrdf(urdf_path)
        self.model_biped_fixed = pin.buildModelFromUrdf(urdf_path)
        
        self.data_biped = self.model_biped.createData()
        self.data_biped_fixed = self.model_biped_fixed.createData()
        
        self.model_nv = self.model_biped.nv
        
        # Initialize matrices
        self.J_l = np.zeros((6, self.model_nv))
        self.J_r = np.zeros((6, self.model_nv))
        self.J_hd_l = np.zeros((6, self.model_nv))
        self.J_hd_r = np.zeros((6, self.model_nv))
        self.J_base = np.zeros((6, self.model_nv))
        self.J_hip_link = np.zeros((6, self.model_nv))
        self.dJ_l = np.zeros((6, self.model_nv))
        self.dJ_r = np.zeros((6, self.model_nv))
        self.dJ_hd_l = np.zeros((6, self.model_nv))
        self.dJ_hd_r = np.zeros((6, self.model_nv))
        self.dJ_base = np.zeros((6, self.model_nv))
        
        self.q = np.zeros(self.model_biped.nq)
        self.dq = np.zeros(self.model_nv)
        self.ddq = np.zeros(self.model_nv)
        self.Rcur = np.eye(3)
        
        self.dyn_M = np.zeros((self.model_nv, self.model_nv))
        self.dyn_M_inv = np.zeros((self.model_nv, self.model_nv))
        self.dyn_C = np.zeros((self.model_nv, self.model_nv))
        self.dyn_G = np.zeros((self.model_nv, 1))
        
        # Get joint indices
        self.r_ankle_joint = self.model_biped.getJointId("J_ankle_r_roll")
        self.l_ankle_joint = self.model_biped.getJointId("J_ankle_l_roll")
        self.r_hand_joint = self.model_biped.getJointId("J_arm_r_07")
        self.l_hand_joint = self.model_biped.getJointId("J_arm_l_07")
        self.r_hand_joint_fixed = self.model_biped_fixed.getJointId("J_arm_r_07")
        self.l_hand_joint_fixed = self.model_biped_fixed.getJointId("J_arm_l_07")
        self.r_hip_joint = self.model_biped.getJointId("J_hip_r_yaw")
        self.l_hip_joint = self.model_biped.getJointId("J_hip_l_yaw")
        self.r_hip_roll_joint = self.model_biped.getJointId("J_hip_r_roll")
        self.l_hip_roll_joint = self.model_biped.getJointId("J_hip_l_roll")
        self.r_ankle_joint_fixed = self.model_biped_fixed.getJointId("J_ankle_r_roll")
        self.l_ankle_joint_fixed = self.model_biped_fixed.getJointId("J_ankle_l_roll")
        self.r_hip_joint_fixed = self.model_biped_fixed.getJointId("J_hip_r_yaw")
        self.l_hip_joint_fixed = self.model_biped_fixed.getJointId("J_hip_l_yaw")
        self.base_joint = self.model_biped.getJointId("root_joint")
        self.waist_yaw_joint = self.model_biped.getJointId("J_waist_yaw")
        
        # Read joint parameters
        with open("E:\\0001_Work_New\\005_YangGu\\0001_ROBOT\\00004_RobotSim2Real\\Isaacsim\\work\\user_examples\\openloong\\common\\joint_ctrl_config.json", "r") as f:
            config = json.load(f)
        
        for i, motor in enumerate(self.motorName):
            self.motorMaxTorque[i] = config[motor]["maxTorque"]
            self.motorMaxPos[i] = config[motor]["maxPos"]
            self.motorMinPos[i] = config[motor]["minPos"]
        
        self.motorReachLimit = [False] * len(self.motorName)
             
    def dataBusRead(self, robotState):
        self.q = robotState.q
        self.dq = robotState.dq
        self.dq[:3] = robotState.base_rot.T @ self.dq[:3]
        self.dq[3:6] = robotState.base_rot.T @ self.dq[3:6]
        self.ddq = robotState.ddq

    def dataBusWrite(self, robotState):
        robotState.J_l = self.J_l
        robotState.J_r = self.J_r
        robotState.J_base = self.J_base
        robotState.dJ_l = self.dJ_l
        robotState.dJ_r = self.dJ_r
        robotState.J_hd_l = self.J_hd_l
        robotState.J_hd_r = self.J_hd_r
        robotState.dJ_hd_l = self.dJ_hd_l
        robotState.dJ_hd_r = self.dJ_hd_r
        robotState.dJ_base = self.dJ_base
        robotState.J_hip_link = self.J_hip_link
        robotState.fe_l_pos_W = self.fe_l_pos
        robotState.fe_r_pos_W = self.fe_r_pos
        robotState.fe_l_pos_L = self.fe_l_pos_body
        robotState.fe_r_pos_L = self.fe_r_pos_body
        robotState.fe_l_rot_W = self.fe_l_rot
        robotState.fe_r_rot_W = self.fe_r_rot
        robotState.fe_l_rot_L = self.fe_l_rot_body
        robotState.fe_r_rot_L = self.fe_r_rot_body
        robotState.hip_r_pos_L = self.hip_r_pos_body
        robotState.hip_l_pos_L = self.hip_l_pos_body
        robotState.hip_r_pos_W = self.hip_r_pos
        robotState.hip_l_pos_W = self.hip_l_pos
        robotState.hd_l_pos_L = self.hd_l_pos_body
        robotState.hd_l_rot_L = self.hd_l_rot_body
        robotState.hd_l_pos_W = self.hd_l_pos
        robotState.hd_l_rot_W = self.hd_l_rot
        robotState.hd_r_pos_L = self.hd_r_pos_body
        robotState.hd_r_rot_L = self.hd_r_rot_body
        robotState.hd_r_pos_W = self.hd_r_pos
        robotState.hd_r_rot_W = self.hd_r_rot
        robotState.hip_link_pos = self.hip_link_pos
        robotState.hip_link_rot = self.hip_link_rot
        
        robotState.dyn_M = self.dyn_M
        robotState.dyn_M_inv = self.dyn_M_inv
        robotState.dyn_C = self.dyn_C
        robotState.dyn_G = self.dyn_G
        robotState.dyn_Ag = self.dyn_Ag
        robotState.dyn_dAg = self.dyn_dAg
        robotState.dyn_Non = self.dyn_Non
        
        robotState.pCoM_W = self.CoM_pos
        robotState.Jcom_W = self.Jcom
        
        robotState.inertia = self.inertia

    def computeJ_dJ(self):
        # pin.forwardKinematics(self.model_biped, self.data_biped, self.q)
        # pin.computeJointJacobians(self.model_biped, self.data_biped, self.q)
        # pin.computeJointJacobiansTimeVariation(self.model_biped, self.data_biped, self.q, self.dq)
        # pin.updateFramePlacements(self.model_biped, self.data_biped)
        
        pin.forwardKinematics(self.model_biped, self.data_biped, self.q)
        pin.jacobianCenterOfMass(self.model_biped, self.data_biped, self.q, True)
        pin.computeJointJacobiansTimeVariation(self.model_biped, self.data_biped, self.q, self.dq)
        pin.updateGlobalPlacements(self.model_biped, self.data_biped)

        pin.getJointJacobian(self.model_biped, self.data_biped, self.r_ankle_joint, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)  ##, self.J_r)
        pin.getJointJacobian(self.model_biped, self.data_biped, self.l_ankle_joint, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)  ##, self.J_l)
        pin.getJointJacobian(self.model_biped, self.data_biped, self.r_hand_joint, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)  ##, self.J_hd_r)
        pin.getJointJacobian(self.model_biped, self.data_biped, self.l_hand_joint, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)  ##, self.J_hd_l)
        pin.getJointJacobian(self.model_biped, self.data_biped, self.base_joint, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)  ##, self.J_base)

        J_hip_roll_l = np.zeros((6, self.model_nv))
        J_hip_roll_r = np.zeros((6, self.model_nv))
        pin.getJointJacobian(self.model_biped, self.data_biped, self.l_hip_roll_joint, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)  ##, self.J_hip_roll_l)
        pin.getJointJacobian(self.model_biped, self.data_biped, self.r_hip_roll_joint, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)  ##, self.J_hip_roll_r)
        
        pin.getJointJacobian(self.model_biped, self.data_biped, self.waist_yaw_joint, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)  ##, self.self.J_hip_link)

        pin.getJointJacobianTimeVariation(self.model_biped, self.data_biped, self.r_ankle_joint, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)  ##, self.dJ_r)
        pin.getJointJacobianTimeVariation(self.model_biped, self.data_biped, self.l_ankle_joint, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)  ##, self.dJ_l)
        pin.getJointJacobianTimeVariation(self.model_biped, self.data_biped, self.r_hand_joint, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)  ##, self.dJ_hd_r)
        pin.getJointJacobianTimeVariation(self.model_biped, self.data_biped, self.l_hand_joint, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)  ##, self.dJ_hd_l)
        pin.getJointJacobianTimeVariation(self.model_biped, self.data_biped, self.base_joint, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)  ##, self.dJ_base)

        self.fe_l_pos = self.data_biped.oMi[self.l_ankle_joint].translation
        self.fe_l_rot = self.data_biped.oMi[self.l_ankle_joint].rotation
        self.hip_l_pos = self.data_biped.oMi[self.l_hip_joint].translation
        self.fe_r_pos = self.data_biped.oMi[self.r_ankle_joint].translation
        self.fe_r_rot = self.data_biped.oMi[self.r_ankle_joint].rotation
        self.hip_r_pos = self.data_biped.oMi[self.r_hip_joint].translation
        self.base_pos = self.data_biped.oMi[self.base_joint].translation
        self.base_rot = self.data_biped.oMi[self.base_joint].rotation
        self.hd_l_pos = self.data_biped.oMi[self.l_hand_joint].translation
        self.hd_l_rot = self.data_biped.oMi[self.l_hand_joint].rotation
        self.hd_r_pos = self.data_biped.oMi[self.r_hand_joint].translation
        self.hd_r_rot = self.data_biped.oMi[self.r_hand_joint].rotation
        self.hip_link_pos = self.data_biped.oMi[self.waist_yaw_joint].translation
        self.hip_link_rot = self.data_biped.oMi[self.waist_yaw_joint].rotation
        self.Jcom = self.data_biped.Jcom

        Mpj = np.eye(self.model_nv)
        Mpj[:3, :3] = self.base_rot.T
        Mpj[3:6, 3:6] = self.base_rot.T
        self.J_l = self.J_l @ Mpj
        self.J_r = self.J_r @ Mpj
        self.J_base = self.J_base @ Mpj
        self.dJ_l = self.dJ_l @ Mpj
        self.dJ_r = self.dJ_r @ Mpj
        self.J_hd_l = self.J_hd_l @ Mpj
        self.J_hd_r = self.J_hd_r @ Mpj
        self.dJ_hd_l = self.dJ_hd_l @ Mpj
        self.dJ_hd_r = self.dJ_hd_r @ Mpj
        self.dJ_base = self.dJ_base @ Mpj
        self.J_hip_link = self.J_hip_link @ Mpj
        self.Jcom = self.Jcom @ Mpj

        q_fixed = self.q[7:7+self.model_biped_fixed.nv]
        pin.forwardKinematics(self.model_biped_fixed, self.data_biped_fixed, q_fixed)
        pin.updateGlobalPlacements(self.model_biped_fixed, self.data_biped_fixed)
        self.fe_l_pos_body = self.data_biped_fixed.oMi[self.l_ankle_joint_fixed].translation
        self.fe_r_pos_body = self.data_biped_fixed.oMi[self.r_ankle_joint_fixed].translation
        self.fe_l_rot_body = self.data_biped_fixed.oMi[self.l_ankle_joint_fixed].rotation
        self.fe_r_rot_body = self.data_biped_fixed.oMi[self.r_ankle_joint_fixed].rotation
        self.hip_l_pos_body = self.data_biped_fixed.oMi[self.l_hip_joint_fixed].translation
        self.hip_r_pos_body = self.data_biped_fixed.oMi[self.r_hip_joint_fixed].translation
        self.hd_l_pos_body = self.data_biped_fixed.oMi[self.l_hand_joint_fixed].translation
        self.hd_l_rot_body = self.data_biped_fixed.oMi[self.l_hand_joint_fixed].rotation
        self.hd_r_pos_body = self.data_biped_fixed.oMi[self.r_hand_joint_fixed].translation
        self.hd_r_rot_body = self.data_biped_fixed.oMi[self.r_hand_joint_fixed].rotation

    @staticmethod
    def quaternion_to_rotation_matrix(q):
        x, y, z, w = q
        return np.array([
            [1 - 2*y*y - 2*z*z, 2*x*y - 2*z*w, 2*x*z + 2*y*w],
            [2*x*y + 2*z*w, 1 - 2*x*x - 2*z*z, 2*y*z - 2*x*w],
            [2*x*z - 2*y*w, 2*y*z + 2*x*w, 1 - 2*x*x - 2*y*y]
        ])

    @staticmethod
    def rotation_matrix_to_quaternion(R):
        trace = np.trace(R)
        if trace > 0:
            S = np.sqrt(trace + 1.0) * 2
            w = 0.25 * S
            x = (R[2, 1] - R[1, 2]) / S
            y = (R[0, 2] - R[2, 0]) / S
            z = (R[1, 0] - R[0, 1]) / S
        elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
            S = np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2
            w = (R[2, 1] - R[1, 2]) / S
            x = 0.25 * S
            y = (R[0, 1] + R[1, 0]) / S
            z = (R[0, 2] + R[2, 0]) / S
        elif R[1, 1] > R[2, 2]:
            S = np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2
            w = (R[0, 2] - R[2, 0]) / S
            x = (R[0, 1] + R[1, 0]) / S
            y = 0.25 * S
            z = (R[1, 2] + R[2, 1]) / S
        else:
            S = np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2
            w = (R[1, 0] - R[0, 1]) / S
            x = (R[0, 2] + R[2, 0]) / S
            y = (R[1, 2] + R[2, 1]) / S
            z = 0.25 * S
        return np.array([x, y, z, w])
    
    @staticmethod
    def intQuat(self, quat, w):
        Rcur = self.quaternion_to_rotation_matrix(quat)
        Rinc = np.eye(3)
        theta = np.linalg.norm(w)
        if theta > 1e-8:
            w_norm = w / theta
            a = np.array([
                [0, -w_norm[2], w_norm[1]],
                [w_norm[2], 0, -w_norm[0]],
                [-w_norm[1], w_norm[0], 0]
            ])
            Rinc = np.eye(3) + a * np.sin(theta) + np.matmul(a, a) * (1 - np.cos(theta))
        Rend = np.matmul(Rcur, Rinc)
        return self.rotation_matrix_to_quaternion(Rend)

    def integrateDIY(self, qI, dqI):
        qRes = np.zeros(self.model_nv + 1)
        wDes = dqI[3:6]
        quatNow = qI[3:7]
        quatNew = self.intQuat(self, quatNow, wDes)
        
        qRes[:] = qI
        qRes[:3] += dqI[:3]
        qRes[3:7] = quatNew
        qRes[7:] += dqI[6:]
        
        return qRes

    
    
    # def intQuat(quat, w):
    #     r = R.from_quat(quat)
    #     Rcur = r.as_matrix()
    #     w_norm = w / np.linalg.norm(w)
    #     if np.linalg.norm(w) > 1e-8:
    #         a = np.array([[0, -w_norm[2], w_norm[1]],
    #                     [w_norm[0], 0, -w_norm[0]],
    #                     [-w_norm[1], w_norm[0], 0]])
    #         theta = np.linalg.norm(w)
    #         Rinc = np.eye(3) + a * np.sin(theta) + np.dot(a, a) * (1 - np.cos(theta))
    #     else:
    #         Rinc = np.eye(3)
    #     Rend = np.dot(Rcur, Rinc)
    #     quat_res = R.from_matrix(Rend).as_quat()
    #     return quat_res

    # def integrateDIY(self, qI, dqI):
    #     qRes = np.zeros(self.model_nv + 1)
    #     wDes = dqI[3:6]
    #     quatNow = qI[3:7]
    #     quatNew = self.intQuat(quatNow, wDes)
    #     qRes = qI.copy()
    #     qRes[:3] += dqI[:3]
    #     qRes[3:7] = quatNew
    #     qRes[7:] += dqI[6:]
    #     return qRes

    def computeDyn(self):
        pin.crba(self.model_biped, self.data_biped, self.q)
        self.dyn_M = self.data_biped.M
        
        pin.computeMinverse(self.model_biped, self.data_biped, self.q)
        self.dyn_M_inv = self.data_biped.Minv
        
        pin.computeCoriolisMatrix(self.model_biped, self.data_biped, self.q, self.dq)
        self.dyn_C = self.data_biped.C
        
        pin.computeGeneralizedGravity(self.model_biped, self.data_biped, self.q)
        self.dyn_G = self.data_biped.g
        
        pin.dccrba(self.model_biped, self.data_biped, self.q, self.dq)
        pin.computeCentroidalMomentum(self.model_biped, self.data_biped, self.q, self.dq)
        self.dyn_Ag = self.data_biped.Ag
        self.dyn_dAg = self.data_biped.dAg
        
        self.dyn_Non = self.dyn_C @ self.dq + self.dyn_G
        
        pin.ccrba(self.model_biped, self.data_biped, self.q, self.dq)
        self.inertia = self.data_biped.Ig.inertia
        
        self.CoM_pos = pin.centerOfMass(self.model_biped, self.data_biped, self.q)
        
        Mpj = np.eye(self.model_nv)
        Mpj_inv = np.eye(self.model_nv)
        Mpj[:3, :3] = self.base_rot.T
        Mpj[3:6, 3:6] = self.base_rot.T
        Mpj_inv[:3, :3] = self.base_rot
        Mpj_inv[3:6, 3:6] = self.base_rot
        
        self.dyn_M = Mpj_inv @ self.dyn_M @ Mpj
        self.dyn_M_inv = Mpj_inv @ self.dyn_M_inv @ Mpj
        self.dyn_C = Mpj_inv @ self.dyn_C @ Mpj
        self.dyn_G = Mpj_inv @ self.dyn_G
        self.dyn_Non = Mpj_inv @ self.dyn_Non

    def computeInK_Leg(self, Rdes_L, Pdes_L, Rdes_R, Pdes_R):
        oMdesL = pin.SE3(Rdes_L, Pdes_L)
        oMdesR = pin.SE3(Rdes_R, Pdes_R)
        qIk = np.zeros(self.model_biped_fixed.nv)
        qIk[22] = -0.1
        qIk[28] = -0.1

        eps = 1e-4
        IT_MAX = 100
        DT = 7e-1
        damp = 5e-3
        
        success = False
        
        J_Idx_l = self.l_ankle_joint_fixed
        J_Idx_r = self.r_ankle_joint_fixed
        
        for itr_count in range(IT_MAX):
            pin.forwardKinematics(self.model_biped_fixed, self.data_biped_fixed, qIk)
            iMdL = self.data_biped_fixed.oMi[J_Idx_l].actInv(oMdesL)
            iMdR = self.data_biped_fixed.oMi[J_Idx_r].actInv(oMdesR)
            errL = pin.log(iMdL).vector
            errR = pin.log(iMdR).vector
            errCompact = np.concatenate([errL, errR])
            
            if np.linalg.norm(errCompact) < eps:
                success = True
                break
            
            JL = pin.computeFrameJacobian(self.model_biped_fixed, self.data_biped_fixed, qIk, J_Idx_l)
            JR = pin.computeFrameJacobian(self.model_biped_fixed, self.data_biped_fixed, qIk, J_Idx_r)
            
            W = np.eye(self.model_biped_fixed.nv)
            JL[:, 16:19] = 0
            JR[:, 16:19] = 0
            
            JlogL = pin.Jlog6(iMdL.inverse())
            JlogR = pin.Jlog6(iMdR.inverse())
            JL = -JlogL @ JL
            JR = -JlogR @ JR
            
            JCompact = np.vstack([JL, JR])
            JJt = JCompact @ W @ JCompact.T
            np.fill_diagonal(JJt, JJt.diagonal() + damp)
            v = -W @ JCompact.T @ np.linalg.solve(JJt, errCompact)
            qIk = pin.integrate(self.model_biped_fixed, qIk, v * DT)
        
        
        
        res = IkRes()
        res.err = errCompact
        res.itr = itr_count
        res.status = 0 if success else -1
        res.jointPosRes = qIk
        
        return res

    def computeInK_Hand(self, Rdes_L, Pdes_L, Rdes_R, Pdes_R):
        oMdesL = pin.SE3(Rdes_L, Pdes_L)
        oMdesR = pin.SE3(Rdes_R, Pdes_R)
        qIk = np.zeros(self.model_biped_fixed.nv)
        
        # Initial guess
        qIk[0:7] = [0.433153883479341, 1.11739345867607, 1.88491913406236,
                    0.802378252758275, 1.22726400279662, 0.0249797771339966, -0.0875282610654057]
        qIk[7:14] = [-0.433152540054138, -1.11739347975224, -1.88492038240761,
                     0.802375980602373, -1.22726323451626, 0.0249795712262396, 0.0875271396314979]

        eps = 1e-4
        IT_MAX = 100
        DT = 6e-1
        damp = 1e-2

        JL = np.zeros((6, self.model_biped_fixed.nv))
        JR = np.zeros((6, self.model_biped_fixed.nv))
        JCompact = np.zeros((12, self.model_biped_fixed.nv))

        success = False
        J_Idx_l = self.l_hand_joint_fixed
        J_Idx_r = self.r_hand_joint_fixed

        for itr_count in range(IT_MAX):
            pin.forwardKinematics(self.model_biped_fixed, self.data_biped_fixed, qIk)
            iMdL = self.data_biped_fixed.oMi[J_Idx_l].actInv(oMdesL)
            iMdR = self.data_biped_fixed.oMi[J_Idx_r].actInv(oMdesR)
            
            errL = pin.log(iMdL).vector
            errR = pin.log(iMdR).vector
            errCompact = np.concatenate((errL, errR))

            if np.linalg.norm(errCompact) < eps:
                success = True
                break

            # pin.computeJointJacobian(self.model_biped_fixed, self.data_biped_fixed, qIk, JL, J_Idx_l)
            pin.computeJointJacobian(self.model_biped_fixed, self.data_biped_fixed, qIk, J_Idx_l)
            # pin.computeJointJacobian(self.model_biped_fixed, self.data_biped_fixed, qIk, J_Idx_r, JR)
            pin.computeJointJacobian(self.model_biped_fixed, self.data_biped_fixed, qIk, J_Idx_r)

            JlogL = pin.Jlog6(iMdL.inverse())
            JlogR = pin.Jlog6(iMdR.inverse())
            JL = -JlogL @ JL
            JR = -JlogR @ JR

            JCompact[:6, :] = JL
            JCompact[6:, :] = JR

            JJt = JCompact @ JCompact.T
            np.fill_diagonal(JJt, JJt.diagonal() + damp)
            
            v = -JCompact.T @ np.linalg.solve(JJt, errCompact)
            qIk = pin.integrate(self.model_biped_fixed, qIk, v * DT)

        res = IkRes()
        res.err = errCompact
        res.itr = itr_count
        res.status = 0 if success else -1
        res.jointPosRes = qIk

        return res

    def workspaceConstraint(self, qFT, tauJointFT):
        for i in range(len(self.motorName)):
            if qFT[i+7] > self.motorMaxPos[i]:
                qFT[i+7] = self.motorMaxPos[i]
                self.motorReachLimit[i] = True
                tauJointFT[i] = self.tauJointOld[i]
            elif qFT[i+7] < self.motorMinPos[i]:
                qFT[i+7] = self.motorMinPos[i]
                self.motorReachLimit[i] = True
                tauJointFT[i] = self.tauJointOld[i]
            else:
                self.motorReachLimit[i] = False

        self.tauJointOld = tauJointFT.copy()